#ifndef _RMRDRIVE_
#define _RMRDRIVE_

#include "RMRPIDIO.h"
#include "RMRAccel.h"
#include "RMRIO.h"
#include "WPIlib.h"

/*
 * RMRDrive.h
 * 
 * Declaration of class RMRDrive
 * 
 * Handles the driving aspect of the robot
 */


// constants, put here because normal static const's can't refer to each other
#define FRONT_CLICKS_PER_REV 360
#define FRONT_ENCODER_RATIO (10.0 / 15.0)
#define FRONT_CLICK_MULTIPLIER 1.0
#define FRONT_ENCODER_SCALING (FRONT_ENCODER_RATIO / (FRONT_CLICK_MULTIPLIER * FRONT_CLICKS_PER_REV))

#define BACK_CLICKS_PER_REV 360
#define BACK_ENCODER_RATIO (10.0 / 22.0)
#define BACK_CLICK_MULTIPLIER 1.0
#define BACK_ENCODER_SCALING (BACK_ENCODER_RATIO / (BACK_CLICK_MULTIPLIER * BACK_CLICKS_PER_REV))


#define MAX_ROBOT_SPEED 5.0



class RMRDrive
{
public:
	// constructor
	RMRDrive(Watchdog &watchdog);
	
	/*
	 * 
	 * IO functions:
	 * 
	 * IO() -- handles joystick button presses, etc
	 * AccelLog(FILE *f) -- debug output of accelerometers to a file
	 * GetPot()	-- old function to get the pot value
	 * 
	 */
	
	// communicate with the joystick
	// current functionality:
	// trigger button resets encoders and PID
	void IO(void);
	
	// log the current accelerometer x anc y voltages to a file
	void AccelLog(FILE* f);
	
	//Calibrate the left and right steering positions from limit switches
	void CalibrateSteering(void);
	
	/*
	 * Control functions:
	 * 
	 * ResetEncoders() -- reset the encoders to zero distance
	 * KillAdvancedControls() -- stop all the fancy driving stuff
	 * DisableTractionControl() -- turn off acceleration limits
	 * EnableTractionControl() -- turn on acceleration limits
	 * DisablePIDs() -- disable all motor PID controls
	 * EnablePIDs() -- enable all motor PID controls
	 */

	
	// stop the pids, encoders, acceleration limits
	void KillAdvancedControls();
	
	// reset the 4 drive encoder distances to zero
	void ResetEncoders(void);
	
	// turn off acceleration limits in the wheels and the accelerometer 
	void DisableTractionControl();
	
	// turn on acceleration limits in the wheels and the accelerometer 
	void EnableTractionControl();
	
	// disable all drive motor PID control (doesn't affect steering)
	void DisableDrivePIDs();
	
	// Enable all motor PID control, including steering
	void EnablePIDs();
	
	/*
	 * Diagnostic tools:
	 * 
	 * Call these methods once each loop to get diagnostic tools
	 * 
	 * ShowEncoderDiagnostics() -- print out encoder position and speed info
	 * TestJoyButtons() -- print out which joystick button is pressed
	 * TestMotors() -- cycle through all the motors, applying power to each in turn
	 */
	
	// print out encoder position and speed info
	void ShowEncoderDiagnostics();
	
	// print out which joystick button is pressed
	void TestJoyButtons(Joystick &joystick);
	
	// cycle through all the motors, applying power to each in turn
	void TestMotors();
	
	
	/*
	 * Driving routines:
	 * 
	 * TankDrive2 				(basic tank drive with PID, controlled by two joysticks -- we uses this to test PID on the kitbot)
	 * CarDrive 				(main drive function, controls the robot via CarDrive from a joystick)
	 * CarDrive(speed, turn)	(do the math for CarDrive)
	 * 
	 * SetPosition				(old, pre PID steering routine)
	 * 
	 */
	
	// PID controlled tank drive
	// two joysticks set the left and right setpoints based on Y axis
	void Tank_Drive2();
	
	// car drive the robot from 1 joystick: Y axis is speed, X axis is turn
	void Car_Drive();
	
	// turn at a desired speed and turn, controlling all 4 motors individually
	void Car_Drive_Individual(float speed, float turn);
	
	// limit wheel output to prevent slipping
	float AntiSlipWheel(float wheel_encoder_speed, float wheel_accel_speed, float wheel_desired_speed);
	
	// this function drives the motors to travel at the desired speed  at the desired heading
	// speed is in the range MIN_SPEED to MAX_SPEED and turn is in the range -1 .. 1
	void Car_Drive(float speed, float turn);
	
	// pre-PID function to have the pot steer the motors
	void SetPosition(Jaguar &motor, float position, float current_position);
	
	

	
	/*
	 * Constants...
	 */
	
	// whether the encoders are reversed
	static const bool LEFT_FRONT_ENCODER_REVERSED = 	false;
	static const bool RIGHT_FRONT_ENCODER_REVERSED = 	true;
	static const bool LEFT_BACK_ENCODER_REVERSED = 		false;
	static const bool RIGHT_BACK_ENCODER_REVERSED = 	true;
	
	static const bool LEFT_FRONT_MOTOR_INVERTED = 		false;
	static const bool RIGHT_FRONT_MOTOR_INVERTED = 		false;
	static const bool LEFT_BACK_MOTOR_INVERTED = 		false;
	static const bool RIGHT_BACK_MOTOR_INVERTED = 		false;
	
	static const bool STEER_MOTOR_INVERTED = 			true;
	
	// PID tuning constants
	static const float FRONT_PID_K_P = 0.25;
	static const float FRONT_PID_K_D = 0.0;
	static const float FRONT_PID_K_I = 0.0;

	static const float BACK_PID_K_P = 0.25;
	static const float BACK_PID_K_D = 0.0;
	static const float BACK_PID_K_I = 0.0;

	// PID tuning constants for steering
	static const float STEER_PID_K_P = 0.75;
	static const float STEER_PID_K_D = 0.01;
	static const float STEER_PID_K_I = 0.01;
	
	
	// digital absolute encoder configuration
	static const INT32 ANGLE_MAX = 274;
	static const INT32 ANGLE_MIN = 640;
	static const INT32 ANGLE_IS_BACKWARDS = true;
	
	
	// max allowed difference between encoder speed and accelerometer speed (in rev/sec)
	//static const float ALLOWED_SLIP = 1.0;

	// accelerometer configuration
	// zero point for accelerometer (should be about 1.5 v)
    static const float ACC_ZERO = 1.4799;
    // sensitivity (should be about 300 g/mV = 0.3)
	static const float ACC_SENSITIVITY = .3;
	
	
	// limit the speed of the robot (by capping max joystick input and max PID input)
	static const float MIN_SPEED = -MAX_ROBOT_SPEED; // from #define
	static const float MAX_SPEED = MAX_ROBOT_SPEED; // from #define
	
	// limit the turning of the robot (the max turn sent from a joystick all the way to the side
	static const float MIN_TURN = -0.75;
	static const float MAX_TURN = 0.75;
	
	// whether turning slows down / speeds up the sides
	static const bool ENABLE_TURN_SLOWDOWN = true;
	// ratio of slowdown on a side to turn (if we turn left, slow the left side to help us turn)
	static const float TURNING_SIDE_SLOWDOWN_FACTOR = 0.3;

	
	// whether we control all 4 motors individually
	static const bool CAR_DRIVE_INDIVIDUAL = true;
	
	// whether we do the anti slip calculation
	static const bool ANTI_SLIP_ENABLED = false;
	
	// How many inches in one complete wheel revolution?
	static const float INCHES_PER_REV = 18.85;
	
	
	static const float WHEEL_BASE = 21;
	static const float WHEEL_TRACK = 21;
	
	static const float ACCEL_DISTANCE_FROM_BACK = 18.5;
	
	//front and back break enable/disable
	static const bool BRAKE_ENABLED = true;
	
	static const CounterBase::EncodingType FRONT_LEFT_ENCODER_TYPE 	= CounterBase::k4X;
	static const CounterBase::EncodingType FRONT_RIGHT_ENCODER_TYPE = CounterBase::k4X;
	static const CounterBase::EncodingType BACK_LEFT_ENCODER_TYPE 	= CounterBase::k4X;
	static const CounterBase::EncodingType BACK_RIGHT_ENCODER_TYPE 	= CounterBase::k4X;
private:
	
	
	/*
	 * private member objects
	 * used by RMRDrive to do the driving
	 */

	PIDAbsoluteEncoder steer_abs_encoder;
	
	PIDJaguar steer_motor;
	
	PIDJaguar left_front_motor;
	PIDJaguar right_front_motor;
	PIDJaguar left_back_motor;
	PIDJaguar right_back_motor;

	PIDEncoder left_front_encoder;
	PIDEncoder right_front_encoder;
	PIDEncoder left_back_encoder;
	PIDEncoder right_back_encoder;
	
	PIDController left_front_pid;
	PIDController right_front_pid;
	PIDController left_back_pid;
	PIDController right_back_pid;
	PIDController steer_pid;
	
	RMRAccel acc_x, acc_y;
	
	bool accelerometer_enabled;
	
	// reference to RMRMain's watchdog
	Watchdog &m_watchdog;
	
	// limit switches for steering
	DigitalInput m_left_limit_switch; 
	DigitalInput m_right_limit_switch; 
	

public:
	
	/*
	 * variables for sharing...
	 * for instance, RMRDrive needs access to the joysticks to call diagnostics
	 */

	Joystick steering_wheel;
	Joystick joystick2;
	
	
	
	

};

#endif
